#include "Tracking.h"

using namespace std;

namespace msf
{
/**
 * @brief 跟踪线程构造函数
 * @param pSys 系统类指针
 * @param pPoseDrawer 画图像的
 * @param sensor 传感器类型
 */
Tracking::Tracking(System *pSys, MapDrawer *pMapDrawer, Atlas *pAtlas, const int sensor, Settings* settings)
: mState(NO_ODOM_YET), mSensor(sensor), mTrackedPFr(0), mbStep(false), mbOnlyTracking(false), mbMapUpdated(false), mpSystem(pSys), 
  mpViewer(NULL), bStepByStep(false), mpMapDrawer(pMapDrawer), mpAtlas(pAtlas), mnInitialPoseFrameId(0), 
  mbCreatedMap(false), mnFirstPoseFrameId(0), mpLastKeyPoseFrame(static_cast<KeyPoseFrame*>(NULL))
{
      // Step 1 从配置文件中加载轮速计和IMU的参数
      if(settings)
      {
            newParameterLoader(settings);
      }

      // Step 2 ESKF ptr init 标准差
      const double sigma_pv = 100; // 100(90)
      const double sigma_rp = 0.001 * kDegreeToRadian; // 0.001
      const double sigma_yaw = 0.001 * kDegreeToRadian; // 0.001

      // 获取Tbo
      // Sophus::SE3<float>

      eskf_ptr_ = std::make_unique<ESKF>(mpImuCalib->mTbo);
      // P : error state covariance init
      eskf_ptr_->state_ptr_->set_cov(sigma_pv, sigma_pv, sigma_rp, sigma_yaw, 0.02, 0.02);
}

/**
 * @brief 根据参数类读取参数
 * @param settings 参数类
 */
void Tracking::newParameterLoader(Settings *settings) 
{
      //1.读取odom参数
      mMinPoseFrames = 50; //10
      mMaxPoseFrames = settings->fps(); //50

      //2.读取IMU参数
      Sophus::SE3f Tbo = settings->Tbo();
      mInsertKPFsLost = settings->insertKPFsWhenLost();
      mImuFreq = settings->imuFrequency();
      mImuPer = 1.0 / (double) mImuFreq; //0.001;     //TODO: ESTO ESTA BIEN?
      float Ng = settings->noiseGyro();
      float Na = settings->noiseAcc();
      float Ngw = settings->gyroWalk();
      float Naw = settings->accWalk();

      const float sf = sqrt(mImuFreq);
      // Q : process noise covariance init 
      mpImuCalib = new IMU::Calib(Tbo,Ng*sf,Na*sf,Ngw/sf,Naw/sf);

      mpImuPreintegratedFromLastKPF = new IMU::Preintegrated(IMU::Bias(),*mpImuCalib);
}


Tracking::~Tracking()
{

}

/**
 * @brief 将imu数据存放在mlQueueImuData的list链表里
 * @param[in] imuMeasurement 
 */
void Tracking::GrabImuData(const IMU::Point &imuMeasurement)
{
    unique_lock<mutex> lock(mMutexImuQueue);
    mlQueueImuData.push_back(imuMeasurement);
}

/**
 * @brief 输入Odometry数据
 * 1、初始化mCurrentPoseFrame
 * 2、进行tracking过程
 * 输出世界坐标系到该帧odom坐标系的变换矩阵
 * @param odom 
 * @param timestamp 时间戳
 */
Sophus::SE3f Tracking::GrabOdom(const nav_msgs::OdometryConstPtr &odom, const double &timestamp)
{
      // 轮速计 + IMU 模式
      if(mSensor == System::IMU_ODOM)
      {
            mCurrentPoseFrame = PoseFrame(odom, timestamp, &mLastPoseFrame, *mpImuCalib);
      }

      Track();

      // 返回位姿
      return mCurrentPoseFrame.GetPose();
}

/**
 * @brief 跟踪过程，包括恒速模型跟踪、参考关键帧跟踪、局部地图跟踪
 * track包含两部分：估计运动、跟踪局部地图
 * 
 * Step 1：初始化
 * Step 2：跟踪
 * Step 3：记录位姿信息，用于轨迹复现
 */
void Tracking::Track()
{
      if (bStepByStep)
      {
            std::cout << "Tracking: Waiting to the next step" << std::endl;
            while(!mbStep && bStepByStep)
                  usleep(500);
            mbStep = false;
      }

      // 1.如局部建图里认为IMU有问题，重置当前活跃地图
      if(mpLocalOptimizer->mbBadImu)
      {
            cout << "TRACK: Reset map because local mapper set the bad imu flag " << endl;
            mpSystem->ResetActiveMap();
            return;
      }      
      // 2.从Atlas中取出当前active的地图
      Map* pCurrentMap = mpAtlas->GetCurrentMap();
      if(!pCurrentMap)
      {
            cout << "ERROR: There is not an active map in the atlas" << endl;
      }      

      // 3.处理时间戳异常的情况
      if(mState!=NO_ODOM_YET)
      {
            if(mLastPoseFrame.mTimeStamp>mCurrentPoseFrame.mTimeStamp)
            {
                  // 如果当前时间戳比前一帧图像时间戳小，说明出错了,清除imu数据，创建新的子地图
                  cerr << "ERROR: Frame with a timestamp older than previous frame detected!" << endl;
                  unique_lock<mutex> lock(mMutexImuQueue);
                  // 创建新地图
                  CreateMapInAtlas();                  
                  return;
            }
            else if(mCurrentPoseFrame.mTimeStamp>mLastPoseFrame.mTimeStamp+1.0)
            {
                  // 如果当前图像时间戳和前一帧图像时间戳大于1s，说明时间戳明显跳变了
                  if(mpAtlas->isInertial())
                  {
                        if(mpAtlas->isImuInitialized())
                        {
                              cout << "Tracking THREAD: Timestamp jump detected. State set to LOST. Reseting IMU integration..." << endl;
                              // IMU完成第3次初始化（在localoptimizing线程里）
                              if(!pCurrentMap->GetIniertialBA2())
                              {
                                    // 如果当前子图中imu没有经过BA2，重置active地图，也就是之前的数据不要了
                                    mpSystem->ResetActiveMap();
                              }
                              else
                              {
                                    // 如果当前子图中imu进行了BA2，重新创建新的子图，保存当前地图
                                    CreateMapInAtlas();
                              }
                        }
                        else
                        {
                              // 当前地图中imu还没有初始化，重置active地图
                              cout << "Tracking THREAD: Timestamp jump detected, before IMU initialization. Reseting..." << endl;
                              mpSystem->ResetActiveMap();
                        }
                        return;
                  }
                  
            }
      }

      // 4. IMU模式下设置IMU的Bias参数,还要保证上一帧存在
      if ((mSensor == System::IMU_ODOM) && mpLastKeyPoseFrame)
            mCurrentPoseFrame.SetNewBias(mpLastKeyPoseFrame->GetImuBias());  // 使用上一帧的bias作为当前帧的初值

      if(mState==NO_ODOM_YET)
      {
            mState = NOT_INITIALIZED;
      }

      mLastProcessedState=mState;

      // 5. 没有创建地图的情况下对IMU数据进行预积分
      if ((mSensor == System::IMU_ODOM) && !mbCreatedMap)
      {
            // IMU数据进行预积分
            PreintegrateIMU();
      }
      mbCreatedMap = false; // 强制设为false，则每次都会进行IMU预积分，与是否创建地图无关了

      // 地图更新时加锁。保证地图不会发生变化
      unique_lock<mutex> lock(pCurrentMap->mMutexMapUpdate);

      mbMapUpdated = false;

      // 判断地图id是否更新了
      int nCurMapChangeIndex = pCurrentMap->GetMapChangeIndex();
      int nMapChangeIndex = pCurrentMap->GetLastMapChange();
      if(nCurMapChangeIndex>nMapChangeIndex)
      {
            // 检测到地图更新了
            pCurrentMap->SetLastMapChange(nCurMapChangeIndex);
            mbMapUpdated = true;
      }      

      // 6. 初始化
      /*流程：
      *第0帧：System::Track()->没有上一帧，无法预积分->Initialization():无预积分->mState=NOT_INITIALIZED->mLastFrame=Frame->return;
      *第1帧：System::Track()->帧间预积分->Initialization():设定初始位姿、初始关键帧、更新当前帧为上一帧->mState=OK->return;
      *第2帧：System::Track()->帧间预积分->mState=OK->进入正常跟踪
      */
      if(mState == NOT_INITIALIZED)
      {
            if(mSensor == System::IMU_ODOM)
            {
                  OdomInitialization();
            }

            if(mState != OK) //正确初始化，mState = OK
            {
                  //没有成功初始化，直接返回
                  mLastPoseFrame = PoseFrame(mCurrentPoseFrame);
                  return;
            }

            if(mpAtlas->GetAllMaps().size() == 1)
            {
                  // 如果当前地图是第一个地图，记录当前帧id为第一帧
                  mnFirstPoseFrameId = mCurrentPoseFrame.mnId;                  
            }
      }
      else
      {
            // 7. 系统成功初始化，进行具体跟踪过程
            bool bOK;
            
            // State OK
            // Local Optimizing激活. 

            // mbOnlyTracking等于false表示正常模式（ESKF定位+后端优化），mbOnlyTracking等于true表示仅ESKF定位模式
            // tracking 类构造时默认为false。在viewer中有个开关ActivateLocalizationMode，可以控制是否开启mbOnlyTracking
            if(!mbOnlyTracking)
            {
                  // 如果正常跟踪
                  if(mState==OK)
                  {
                        //ESKF定位
                        //TODO： 第一帧是否需要特殊处理
                        cout << "Tracking THREAD: Track with IMU+Odom ESKF " << endl;
                        bOK = TrackWithESKF();
                        
                        if(!bOK)
                        {
                              mState = LOST;
                        }

                  }
                  else // mState == LOST 跟踪不正常按照下面处理
                  {
                        // 开启一个新地图
                        std::cout << "Tracking THREAD: A new map is started..." << std::endl;

                        if (pCurrentMap->KeyPoseFramesInMap()<10)
                        {
                              // 当前地图中关键帧数目小于10，重置当前地图
                              mpSystem->ResetActiveMap();
                              std::cout << "Tracking THREAD: Reseting current map..." << std::endl;
                        }
                        else
                              CreateMapInAtlas();  // 当前地图中关键帧数目超过10，创建新地图                        

                        // 干掉上一个关键帧
                        if(mpLastKeyPoseFrame)
                              mpLastKeyPoseFrame = static_cast<KeyPoseFrame*>(NULL);

                        std::cout << "Tracking THREAD: done" << std::endl;

                        return;
                  }
            }
            else // 纯定位模式
            {
                  // 只进行跟踪tracking，局部地图不工作
                  if(mSensor == System::IMU_ODOM)
                  {
                        bOK = TrackWithESKF();
                  }     
            }

            // 将最新的关键帧作为当前帧的参考关键帧
            // mpReferenceKF先是上一时刻的参考关键帧，如果当前为新关键帧则变成当前关键帧，如果不是新的关键帧则先为上一帧的参考关键帧，而后经过更新局部关键帧重新确定
            if(!mCurrentPoseFrame.mpReferenceKPF)
                  mCurrentPoseFrame.mpReferenceKPF = mpReferenceKPF;

            // 8. 在跟踪得到当前帧初始姿态后，现在对localmap进行tracking，优化当前位姿
            // 前面只是跟踪一帧得到初始位姿，这里搜索局部关键帧和当前帧进行匹配
            if(!mbOnlyTracking)
            {
                  if(bOK)
                  {
                        //局部地图跟踪
                        bOK = TrackLocalOptim();
                  }
                  if(!bOK)
                  {
                        cout << "Tracking THREAD: Fail to track local map!" << endl;
                  }
            }
            
            // 到此为止跟踪确定位姿阶段结束,下面开始做收尾工作和为下一帧做准备

            // 9. 判断是否追踪成功
            if(bOK)
            {
                  // 此时OK说明跟踪成功
                  mState = OK;
            }
            else if(mState == OK) // 只有当第一阶段跟踪成功bOK=1 mState=OK，但第二阶段局部地图跟踪失败bOK=0,mState=OK时执行
            {
                  if(mSensor==System::IMU_ODOM)
                  {
                        std::cout << "Tracking THREAD: Track lost for less than one second..." << std::endl;
                        if(!pCurrentMap->isImuInitialized() || !pCurrentMap->GetIniertialBA2())
                        {
                              // IMU模式下IMU没有成功初始化或者没有完成IMU BA，则重置当前地图
                              mpSystem->ResetActiveMap();
                        }
                        mState = LOST;
                  }
                  else
                  {
                        mState = LOST;
                  }
            }

            // 10. Update drawer
            //更新显示线程中的位姿等信息
            if(mCurrentPoseFrame.isSet())
            {
                  mpMapDrawer->SetCurrentOdomPose(mCurrentPoseFrame.GetPose());
            }
            
            // 11. 如果跟踪成功，更新速度，按需创建关键帧
            if(bOK)
            {
                  // 11.1 更新恒速运动模型TrackWithMotionModel中的mVelocity
                  if(mLastPoseFrame.isSet() && mCurrentPoseFrame.isSet())
                  {
                        Sophus::SE3f LastTwo = mLastPoseFrame.GetPose().inverse();
                        // mVelocity = Tcl = Tcw * Twl,表示上一帧到当前帧的变换， 其中 Twl = LastTwc
                        mVelocity = mCurrentPoseFrame.GetPose() * LastTwo;
                        mbVelocity = true;
                  }
                  else 
                  {
                        // 否则没有速度
                        mbVelocity = false;
                  }

                  // 使用IMU积分的位姿显示
                  if(mSensor == System::IMU_ODOM)
                        mpMapDrawer->SetCurrentOdomPose(mCurrentPoseFrame.GetPose());

                  // 11.2 判断是否需要插入关键帧,局部优化线程会从共享内存中读取最新的位姿关键帧的信息，并根据它们重新计算位姿关键帧
                  bool bNeedKPF = NeedNewKeyPoseFrame();    

                  // 11.3 根据条件来判断是否插入关键帧
                  if(bNeedKPF && (bOK || mSensor==System::IMU_ODOM))
                  {
                        CreateNewKeyPoseFrame();  // 创建关键位姿帧
                  }

            }

            // Step 12 如果第二阶段跟踪失败，跟踪状态为LOST
            if(mState==LOST)
            {
                  // 如果地图中关键帧小于10，重置当前地图，退出当前跟踪
                  if(pCurrentMap->KeyPoseFramesInMap()<=10)  // 上一个版本这里是5
                  {
                        mpSystem->ResetActiveMap();
                        return;
                  }                  
                  if (mSensor == System::IMU_ODOM)
                        if (!pCurrentMap->isImuInitialized())
                        {
                              // 如果是IMU模式并且还未进行IMU初始化，重置当前地图，退出当前跟踪
                              std::cout << "Tracking THREAD: Track lost before IMU initialization, reseting..." << std::endl;
                              mpSystem->ResetActiveMap();
                              return;
                        }

                  // 如果地图中关键帧超过10 并且 IMU模式但是已经完成IMU初始化了，保存当前地图，创建新的地图
                  CreateMapInAtlas();

                  return;
            }

            // 确保已经设置了参考关键帧
            if(!mCurrentPoseFrame.mpReferenceKPF)
                  mCurrentPoseFrame.mpReferenceKPF = mpReferenceKPF;
                  
            // 保存上一帧的数据,当前帧变上一帧
            mLastPoseFrame = PoseFrame(mCurrentPoseFrame);
      }

      // Step 13 记录位姿信息，用于最后保存所有的轨迹
      if(mState==OK)
      {
            if(mCurrentPoseFrame.isSet())
            {
                  // 计算相对姿态Tcr = Tcw * Twr, Twr = Trw^-1
                  Sophus::SE3f Tcr_ = mCurrentPoseFrame.GetPose() * mCurrentPoseFrame.mpReferenceKPF->GetPoseInverse();
                  mlRelativeFramePoses.push_back(Tcr_);
                  mlpReferences.push_back(mCurrentPoseFrame.mpReferenceKPF);
                  mlFrameTimes.push_back(mCurrentPoseFrame.mTimeStamp);
                  mlbLost.push_back(mState==LOST);
            }
            else
            {
                  // 如果跟踪失败，则相对位姿使用上一次值
                  mlRelativeFramePoses.push_back(mlRelativeFramePoses.back());
                  mlpReferences.push_back(mlpReferences.back());
                  mlFrameTimes.push_back(mlFrameTimes.back());
                  mlbLost.push_back(mState==LOST);
            }
      }
}

/**
 * @brief 设置局部建图器
 * @param pLocalMapper 局部地图类
 */
void Tracking::SetLocalOptimizer(LocalOptimizing *pLocalOptimizer)
{
    mpLocalOptimizer=pLocalOptimizer;
}

/**
 * @brief 设置显示器
 * @param pViewer 显示类
 */
void Tracking::SetViewer(Viewer *pViewer)
{
    mpViewer=pViewer;
}

/**
 * @brief 预积分，对于一个帧有两种预积分，一种是相对于上一位姿帧，一种是相对于上一个关键位姿帧
 */
void Tracking::PreintegrateIMU()
{
      // Step 1.拿到两两帧之间待处理的预积分数据，组成一个集合
      // 上一帧不存在,说明两帧之间没有imu数据，不进行预积分
      if(!mCurrentPoseFrame.mpPrevPoseFrame)
      {
            //Verbose::PrintMess("non prev frame ", Verbose::VERBOSITY_NORMAL);
            cout << " non prev poseframe "<<endl;
            mCurrentPoseFrame.setIntegrated();
            return;
      }

      mvImuFromLastPoseFrame.clear();
      mvImuFromLastPoseFrame.reserve(mlQueueImuData.size());
      // 没有imu数据,不进行预积分
      if(mlQueueImuData.size() == 0)
      {
            //Verbose::PrintMess("Not IMU data in mlQueueImuData!!", Verbose::VERBOSITY_NORMAL);
            cout << " Not IMU data in mlQueueImuData!! "<<endl;
            mCurrentPoseFrame.setIntegrated();
            return;
      }

      while(true)
      {
            // 数据还没有时,会等待一段时间,直到mlQueueImuData中有imu数据.一开始不需要等待
            bool bSleep = false;
            {
                  unique_lock<mutex> lock(mMutexImuQueue);
                  if(!mlQueueImuData.empty())
                  {
                        // 拿到第一个imu数据作为起始数据
                        IMU::Point* m = &mlQueueImuData.front();
                        cout.precision(17);
                        // imu起始数据会比当前帧的前一帧时间戳早,如果相差0.001则舍弃这个imu数据
                        if(m->t<mCurrentPoseFrame.mpPrevPoseFrame->mTimeStamp-mImuPer)
                        {
                              mlQueueImuData.pop_front(); //过早，舍弃
                        }
                        // 同样最后一个的imu数据时间戳也不能理当前帧时间间隔多余0.001
                        else if(m->t<mCurrentPoseFrame.mTimeStamp-mImuPer)
                        {
                              mvImuFromLastPoseFrame.push_back(*m); //正常数据，压入，然后pop
                              mlQueueImuData.pop_front();
                        }
                        else
                        {
                              // 得到两帧间的imu数据放入mvImuFromLastPoseFrame中,得到后面预积分的处理数据
                              mvImuFromLastPoseFrame.push_back(*m); //最后数据，压入，然后break退出
                              break;
                        }
                  }
                  else
                  {
                        break;
                        bSleep = true;
                  }
            }
            if(bSleep)
                  usleep(500);
      }      

      // Step 2.对两帧之间进行中值积分处理
      // m个imu组数据会有m-1个预积分量
      const int n = mvImuFromLastPoseFrame.size()-1;
      if(n==0){
            cout << "Empty IMU measurements vector!!!\n";
            return;
      }

      // 构造imu预处理器,并初始化标定数据
      IMU::Preintegrated* pImuPreintegratedFromLastPoseFrame = new IMU::Preintegrated(mLastPoseFrame.mImuBias,mCurrentPoseFrame.mImuCalib);      
      // 针对预积分位置的不同做不同中值积分的处理
      /**
       *  根据上面imu帧的筛选，IMU与Odom帧的时序如下：
       *  Frame---IMU0---IMU1---IMU2---IMU3---IMU4---------------IMUx---Frame---IMUx+1
       *  T_------T0-----T1-----T2-----T3-----T4-----------------Tx-----_T------Tx+1
       *  A_------A0-----A1-----A2-----A3-----A4-----------------Ax-----_T------Ax+1
       *  W_------W0-----W1-----W2-----W3-----W4-----------------Wx-----_T------Wx+1
       *  T_和_T分别表示上一位姿帧和当前位姿帧的时间戳，A(加速度数据)，W(陀螺仪数据)，同理
       */
      for(int i=0; i<n; i++)
      {
            float tstep;
            Eigen::Vector3f acc, angVel;
            // 第一帧数据但不是最后两帧,imu总帧数大于2
            if((i==0) && (i<(n-1)))
            {
                  // 获取相邻两段imu的时间间隔
                  float tab = mvImuFromLastPoseFrame[i+1].t-mvImuFromLastPoseFrame[i].t;
                  // 获取当前imu到上一帧的时间间隔
                  float tini = mvImuFromLastPoseFrame[i].t-mCurrentPoseFrame.mpPrevPoseFrame->mTimeStamp;
                  // 设当前时刻imu的加速度a0，下一时刻加速度a1，时间间隔tab 为t10，tini t0p
                  // 正常情况下时为了求上一帧到当前时刻imu的一个平均加速度，但是imu时间不会正好落在上一帧的时刻，需要做补偿，要求得a0时刻到上一帧这段时间加速度的改变量
                  // 有了这个改变量将其加到a0上之后就可以表示上一帧时的加速度了。其中a0 - (a1-a0)*(tini/tab) 为上一帧时刻的加速度再加上a1 之后除以2就为这段时间的加速度平均值
                  // 其中tstep表示a1到上一帧的时间间隔，a0 - (a1-a0)*(tini/tab)这个式子中tini可以是正也可以是负表示时间上的先后，(a1-a0)也是一样，多种情况下这个式子依然成立
                  acc = (mvImuFromLastPoseFrame[i].a+mvImuFromLastPoseFrame[i+1].a-
                        (mvImuFromLastPoseFrame[i+1].a-mvImuFromLastPoseFrame[i].a)*(tini/tab))*0.5f;
                  // 计算过程类似加速度
                  angVel = (mvImuFromLastPoseFrame[i].w+mvImuFromLastPoseFrame[i+1].w-
                        (mvImuFromLastPoseFrame[i+1].w-mvImuFromLastPoseFrame[i].w)*(tini/tab))*0.5f;
                  tstep = mvImuFromLastPoseFrame[i+1].t-mCurrentPoseFrame.mpPrevPoseFrame->mTimeStamp;
            }
            else if(i<(n-1))
            {
                  // 中间的数据不存在帧的干扰，正常计算
                  acc = (mvImuFromLastPoseFrame[i].a+mvImuFromLastPoseFrame[i+1].a)*0.5f;
                  angVel = (mvImuFromLastPoseFrame[i].w+mvImuFromLastPoseFrame[i+1].w)*0.5f;
                  tstep = mvImuFromLastPoseFrame[i+1].t-mvImuFromLastPoseFrame[i].t;
            }
            // 直到倒数第二个imu时刻时，计算过程跟第一时刻类似，都需要考虑帧与imu时刻的关系
            else if((i>0)&&(i==n-1))
            {
                  float tab = mvImuFromLastPoseFrame[i+1].t-mvImuFromLastPoseFrame[i].t;
                  float tend = mvImuFromLastPoseFrame[i+1].t-mCurrentPoseFrame.mTimeStamp;
                  acc = (mvImuFromLastPoseFrame[i].a+mvImuFromLastPoseFrame[i+1].a-
                        (mvImuFromLastPoseFrame[i+1].a-mvImuFromLastPoseFrame[i].a)*(tend/tab))*0.5f;
                  angVel = (mvImuFromLastPoseFrame[i].w+mvImuFromLastPoseFrame[i+1].w-
                        (mvImuFromLastPoseFrame[i+1].w-mvImuFromLastPoseFrame[i].w)*(tend/tab))*0.5f;
                  tstep = mCurrentPoseFrame.mTimeStamp-mvImuFromLastPoseFrame[i].t;
            }
            // 就两个数据时使用第一个时刻的，这种情况应该没有吧，，回头应该试试看
            else if((i==0) && (i==(n-1)))
            {
                  acc = mvImuFromLastPoseFrame[i].a;
                  angVel = mvImuFromLastPoseFrame[i].w;
                  tstep = mCurrentPoseFrame.mTimeStamp-mCurrentPoseFrame.mpPrevPoseFrame->mTimeStamp;
            }
            // Step 3.依次进行预积分计算
            // 应该是必存在的吧，一个是相对上一关键帧，一个是相对上一帧
            if (!mpImuPreintegratedFromLastKPF)
                  cout << "mpImuPreintegratedFromLastKPF does not exist" << endl;
            mpImuPreintegratedFromLastKPF->IntegrateNewMeasurement(acc,angVel,tstep);
            pImuPreintegratedFromLastPoseFrame->IntegrateNewMeasurement(acc,angVel,tstep);
      }

      // 记录当前预积分的图像帧
      mCurrentPoseFrame.mpImuPreintegratedPoseFrame = pImuPreintegratedFromLastPoseFrame;
      mCurrentPoseFrame.mpImuPreintegrated = mpImuPreintegratedFromLastKPF;
      mCurrentPoseFrame.mpLastKeyPoseFrame = mpLastKeyPoseFrame;

      mCurrentPoseFrame.setIntegrated();
}

/**
 * @brief 跟踪不成功的时候，用初始化好的imu数据做跟踪处理，通过IMU预测状态
 * 两个地方用到：
 * 1. 匀速模型计算速度,但并没有给当前帧位姿赋值；
 * 2. 跟踪丢失时不直接判定丢失，通过这个函数预测当前帧位姿看看能不能拽回来，代替纯视觉中的重定位
 * 
 * @return true 
 * @return false 
 */
bool Tracking::PredictStateIMU()
{
      if(!mCurrentPoseFrame.mpPrevPoseFrame)
      {
            //Verbose::PrintMess("No last frame", Verbose::VERBOSITY_NORMAL);
            cout<< "No last frame" <<endl;
            return false;
      }

      // 总结下都在什么时候地图更新，也就是mbMapUpdated为true
      // 1. 局部地图LocalBundleAdjustment
      // 2. IMU三阶段的初始化
      // 计算时相对的帧不同，地图有更新后相对于上一关键帧做的，反之相对于上一帧
      if(mbMapUpdated && mpLastKeyPoseFrame)
      {
            const Eigen::Vector3f twb1 = mpLastKeyPoseFrame->GetImuPosition();
            const Eigen::Matrix3f Rwb1 = mpLastKeyPoseFrame->GetImuRotation();
            const Eigen::Vector3f Vwb1 = mpLastKeyPoseFrame->GetVelocity();

            const Eigen::Vector3f Gz(0, 0, -IMU::GRAVITY_VALUE);
            const float t12 = mpImuPreintegratedFromLastKPF->dT;

            // 计算当前帧在世界坐标系的位姿,原理都是用预积分的位姿（预积分的值不会变化）与上一帧的位姿（会迭代变化）进行更新 
            // 旋转 R_wb2 = R_wb1 * R_b1b2
            Eigen::Matrix3f Rwb2 = IMU::NormalizeRotation(Rwb1 * mpImuPreintegratedFromLastKPF->GetDeltaRotation(mpLastKeyPoseFrame->GetImuBias()));
            // 位移
            Eigen::Vector3f twb2 = twb1 + Vwb1*t12 + 0.5f*t12*t12*Gz+ Rwb1*mpImuPreintegratedFromLastKPF->GetDeltaPosition(mpLastKeyPoseFrame->GetImuBias());
            // 速度 
            Eigen::Vector3f Vwb2 = Vwb1 + t12*Gz + Rwb1 * mpImuPreintegratedFromLastKPF->GetDeltaVelocity(mpLastKeyPoseFrame->GetImuBias());
            // 设置当前帧的世界坐标系的相机位姿
            mCurrentPoseFrame.SetImuPoseVelocity(Rwb2,twb2,Vwb2);

            // 记录bias
            mCurrentPoseFrame.mImuBias = mpLastKeyPoseFrame->GetImuBias();
            mCurrentPoseFrame.mPredBias = mCurrentPoseFrame.mImuBias;
            return true;
      }  
      else if(!mbMapUpdated)       
      {
            const Eigen::Vector3f twb1 = mLastPoseFrame.GetImuPosition();
            const Eigen::Matrix3f Rwb1 = mLastPoseFrame.GetImuRotation();
            const Eigen::Vector3f Vwb1 = mLastPoseFrame.GetVelocity();
            const Eigen::Vector3f Gz(0, 0, -IMU::GRAVITY_VALUE);
            // mpImuPreintegratedFrame是当前帧上一帧，不一定是关键帧
            const float t12 = mCurrentPoseFrame.mpImuPreintegratedPoseFrame->dT;

            Eigen::Matrix3f Rwb2 = IMU::NormalizeRotation(Rwb1 * mCurrentPoseFrame.mpImuPreintegratedPoseFrame->GetDeltaRotation(mLastPoseFrame.mImuBias));
            Eigen::Vector3f twb2 = twb1 + Vwb1*t12 + 0.5f*t12*t12*Gz+ Rwb1 * mCurrentPoseFrame.mpImuPreintegratedPoseFrame->GetDeltaPosition(mLastPoseFrame.mImuBias);
            Eigen::Vector3f Vwb2 = Vwb1 + t12*Gz + Rwb1 * mCurrentPoseFrame.mpImuPreintegratedPoseFrame->GetDeltaVelocity(mLastPoseFrame.mImuBias);

            mCurrentPoseFrame.SetImuPoseVelocity(Rwb2,twb2,Vwb2);

            mCurrentPoseFrame.mImuBias = mLastPoseFrame.mImuBias;
            mCurrentPoseFrame.mPredBias = mCurrentPoseFrame.mImuBias;
            return true;            
      }
      else
            cout << "not IMU prediction!!" << endl;

      return false;
}

/**
 * @brief Odom初始化
 */
void Tracking::OdomInitialization()
{
      //TODO：首先确认当前odom消息获取到

      if (mSensor == System::IMU_ODOM)
      {
            if (!mCurrentPoseFrame.mpImuPreintegrated || !mLastPoseFrame.mpImuPreintegrated)
            {
                  cout << "Tracking THREAD: not IMU meas" << endl;
                  return;
            }

            if (!mFastInit && (mCurrentPoseFrame.mpImuPreintegratedPoseFrame->avgA-mLastPoseFrame.mpImuPreintegratedPoseFrame->avgA).norm()<0.5)
            {
                  cout << "Tracking THREAD: not enough acceleration" << endl;
                  return;
            }

            if(mpImuPreintegratedFromLastKPF)
                  delete mpImuPreintegratedFromLastKPF;

            mpImuPreintegratedFromLastKPF = new IMU::Preintegrated(IMU::Bias(),*mpImuCalib);
            mCurrentPoseFrame.mpImuPreintegrated = mpImuPreintegratedFromLastKPF;
      }

      // 设定初始位姿为单位旋转，0平移，imu模式下设置的是odom位姿
      if (mSensor == System::IMU_ODOM)
      {
            Eigen::Matrix3f Rwb0 = mCurrentPoseFrame.mImuCalib.mTob.rotationMatrix();
            Eigen::Vector3f twb0 = mCurrentPoseFrame.mImuCalib.mTob.translation();
            Eigen::Vector3f Vwb0;
            Vwb0.setZero();
            mCurrentPoseFrame.SetImuPoseVelocity(Rwb0, twb0, Vwb0);
      }
      else
            mCurrentPoseFrame.SetPose(Sophus::SE3f());

      // 将当前帧构造为初始关键帧
      KeyPoseFrame* pKPFini = new KeyPoseFrame(mCurrentPoseFrame,mpAtlas->GetCurrentMap());

      // Insert KeyFrame in the map
      // 在地图中添加该初始关键帧
      mpAtlas->AddKeyPoseFrame(pKPFini);

      // 在局部优化中添加该初始关键帧
      mpLocalOptimizer->InsertKeyPoseFrame(pKPFini);

      // 更新当前帧为上一帧
      /*
      mLastPoseFrame = PoseFrame(mCurrentPoseFrame);
      mnLastKeyPoseFrameId = mCurrentPoseFrame.mnId;
      mpLastKeyPoseFrame = pKPFini;

      mvpLocalKeyPoseFrames.push_back(pKPFini);
      mpReferenceKPF = pKPFini;
      mCurrentPoseFrame.mpReferenceKPF = pKPFini;
      */

      // fix bug2: 第三个普通帧（第二个关键帧）进行ESKF UpdateLastPoseFrame时，mLastPoseFrame没有mpReferenceKPF
      mvpLocalKeyPoseFrames.push_back(pKPFini);
      mpReferenceKPF = pKPFini;
      mCurrentPoseFrame.mpReferenceKPF = pKPFini;
      mnLastKeyPoseFrameId = mCurrentPoseFrame.mnId;
      mpLastKeyPoseFrame = pKPFini;

      mLastPoseFrame = PoseFrame(mCurrentPoseFrame);

      mpAtlas->GetCurrentMap()->mvpKeyPoseFrameOrigins.push_back(pKPFini);

      mpMapDrawer->SetCurrentOdomPose(mCurrentPoseFrame.GetPose());

      // 追踪成功
      mState=OK;
}

/**
 * @brief 在Atlas中保存当前地图，创建新地图，所有跟状态相关的变量全部重置
 */
void Tracking::CreateMapInAtlas()
{
      mnLastInitPoseFrameId = mCurrentPoseFrame.mnId;
      mpAtlas->CreateNewMap();
      if (mSensor==System::IMU_ODOM)
            mpAtlas->SetInertialSensor();  // mpAtlas中map的mbIsInertial=true
      //mbSetInit=false;  // 好像没什么用

      mnInitialPoseFrameId = mCurrentPoseFrame.mnId+1;
      mState = NO_ODOM_YET;

      // 重置上一个关键帧相关的变量的信息
      mbVelocity = false;
      cout << "Tracking THREAD: First frame id in map: " + to_string(mnLastInitPoseFrameId+1) <<endl;

      if(mSensor == System::IMU_ODOM && mpImuPreintegratedFromLastKPF)
      {
            delete mpImuPreintegratedFromLastKPF;
            mpImuPreintegratedFromLastKPF = new IMU::Preintegrated(IMU::Bias(),*mpImuCalib);
      }

      if(mpLastKeyPoseFrame)
            mpLastKeyPoseFrame = static_cast<KeyPoseFrame*>(NULL);

      if(mpReferenceKPF)
            mpReferenceKPF = static_cast<KeyPoseFrame*>(NULL);

      mLastPoseFrame = PoseFrame();
      mCurrentPoseFrame = PoseFrame();
      mlQueueImuData.clear();

      mbCreatedMap = true;
}

/**
 * @brief 用参考关键帧来对当前普通帧进行跟踪
 */
bool Tracking::TrackReferenceKeyPoseFrame()
{

}

/**
 * @brief 根据小车运动学模型用上一帧来对当前帧进行跟踪
 * Step 1：更新上一帧的位姿
 * Step 2：当前帧位姿
 * @return 跟踪成功，返回true
 */
bool Tracking::TrackWithMotionModel()
{

}

/**
 * @brief 更新上一帧位姿，在上一帧中生成临时地图点
 * 单目情况：只计算了上一帧的世界坐标系位姿
 * 双目和rgbd情况：选取有有深度值的并且没有被选为地图点的点生成新的临时地图点，提高跟踪鲁棒性
 */
void Tracking::UpdateLastPoseFrame()
{
      // Step 1：利用参考关键帧更新上一帧在世界坐标系下的位姿
      // 上一普通帧的参考关键帧，注意这里用的是参考关键帧（位姿准）而不是上上一帧的普通帧
      KeyPoseFrame* pRef = mLastPoseFrame.mpReferenceKPF;
      // ref_keyposeframe 到 lastposeframe的位姿变换
      Sophus::SE3f Tlr = mlRelativeFramePoses.back();
      // 将上一帧的世界坐标系下的位姿计算出来
      // l:last, r:reference, w:world
      // Tlw = Tlr*Trw 

      //Problem3 debug显示mpReferenceKPF中没有值: 见fix bug2
      mLastPoseFrame.SetPose(Tlr * pRef->GetPose());

      // 如果上一帧为关键帧，或者单目/单目惯性，SLAM模式的情况，则退出
      if(mnLastKeyPoseFrameId==mLastPoseFrame.mnId || mSensor==System::IMU_ODOM)
            return;
}

/**
 * @brief 使用ESKF融合IMU和Odom数据，从上一帧来对当前帧进行跟踪
 * Step 1：更新上一帧的位姿
 * Step 2：当前帧位姿
 * @return 跟踪成功，返回true
 */
bool Tracking::TrackWithESKF()
{
      // Step 1：通过参考关键帧信息，更新上一帧的位姿
      UpdateLastPoseFrame(); 

      bool init = mpAtlas->isImuInitialized();
      const int n = mvImuFromLastPoseFrame.size()-1; //std::vector<msf::IMU::Point>

      //std::shared_ptr<msf::IMU::Point> last_imu;
      //std::shared_ptr<msf::IMU::Point> curr_imu;
      // | * * * |
      for(int i=0;i<n;++i)
      {
            //Problem3 Eigen内存对齐与make shared之间的问题
            //IMU::ImuDataPtr last_imu = std::shared_ptr<IMU::Point>(new IMU::Point(mvImuFromLastPoseFrame[i]));
            //IMU::ImuDataPtr curr_imu = std::shared_ptr<IMU::Point>(new IMU::Point(mvImuFromLastPoseFrame[i+1]));

            eskf_ptr_->predict(mvImuFromLastPoseFrame[0], mvImuFromLastPoseFrame[0+1], mLastPoseFrame, init); // last_imu,curr_imu: std::shared_ptr<const msf::IMU::Point>
            init = false; //只有第一次需要上一帧数据
      }

      eskf_ptr_->correct(mCurrentPoseFrame.odom_data); // measurement: std::shared_ptr<const msf::OdomData>

      // 获取旋转矩阵Rwb_和p_wb_
      Eigen::Matrix3d Rwb = eskf_ptr_->state_ptr_->Rwb_;
      Eigen::Vector3d p_wb = eskf_ptr_->state_ptr_->p_wb_;
      Eigen::Vector3d v_wb = eskf_ptr_->state_ptr_->v_wb_;

      // 通过ESKF融合定位得到当前帧的初始位姿和速度。
      //mCurrentPoseFrame.SetPose(Twb.inverse()); // const Sophus::SE3<float> &Tbw
      mCurrentPoseFrame.SetImuPoseVelocity(Rwb.cast<float>(), p_wb.cast<float>(), v_wb.cast<float>()); // Eigen::Matrix3f &Rwb, Eigen::Vector3f &twb, Eigen::Vector3f &Vwb

      return true;
}

/**
 * @brief 用局部地图进行跟踪，进一步优化位姿
 * Step 1：更新局部关键帧mvpLocalKeyFrames
 * Step 2：对位姿再次优化
 * Step 3：决定是否跟踪成功
 * @return true if success
 */
bool Tracking::TrackLocalOptim()
{
      mTrackedPFr++;

      // Step 1：更新局部关键帧 mvpLocalKeyPoseFrames
      UpdateLocalOptim();

      // Step 2：对位姿再次优化
      int inliers;
      // IMU未初始化，无法进行优化
      if (!mpAtlas->isImuInitialized())
            //Problem4 return false导致系统一直重置
            return true;
      else
      {
            // 获取当前地图
            int size = mpAtlas->GetCurrentMap()->KeyPoseFramesInMap();

            if(size<10) //keyposeframe数量太少
            {
                  return false;
            }
            else //如果积累数据量比较多，考虑使用优化
            {
                  // 未更新地图
                  // Problem19 : 过早进行局部优化，完成oiba1之后再进行
                  if(mbMapUpdated && mpAtlas->GetCurrentMap()->GetIniertialBA1())
                  {
                        //std::cout << "Tracking THREAD: TLM: PoseInertialOptimizationLastKeyFrame" << std::endl;
                        // 使用上一关键帧以及当前帧的Odom信息和IMU信息联合优化当前帧位姿、速度和IMU零偏
                        // Problem14.2 轨迹优化漂移，暂时取消当前帧优化
                        //inliers = Optimizer::PoseInertialOptimization(&mCurrentPoseFrame);
                  }
                  //else
                  //{
                        //std::cout << "Tracking THREAD: TLM: No Map Updated!!" << std::endl;
                        // 使用上一普通帧以及当前帧的Odom信息和IMU信息联合优化当前帧位姿、速度和IMU零偏
                        //inliers = Optimizer::PoseInertialOptimizationLastPoseFrame(&mCurrentPoseFrame);
                  //}
            }
      }

      // Step 3：决定是否跟踪成功
      return true;
}

/**
 * @brief 判断当前帧是否需要插入关键帧
 * Step 1：关键帧数目超出最大限制，不插入关键帧
 * Step 2：查询局部地图管理器是否繁忙,也就是当前能否接受新的关键帧
 * Step 3：决策是否需要插入关键帧
 * @return true         需要
 * @return false        不需要
 */
bool Tracking::NeedNewKeyPoseFrame()
{
      // 如果是IMU模式并且当前地图中未完成IMU初始化
      if((mSensor == System::IMU_ODOM) && !mpAtlas->GetCurrentMap()->isImuInitialized())
      {
            if ((mSensor == System::IMU_ODOM) && (mCurrentPoseFrame.mTimeStamp-mpLastKeyPoseFrame->mTimeStamp)>=0.25)
                  return true;
            else
                  return false;
      }

      // Step 1：纯跟踪模式下不插入关键帧
      //if(mbOnlyTracking)
            //return false;

      // Step 2：如果局部地图线程被使用，则不插入关键帧
      if(mpLocalOptimizer->isStopped() || mpLocalOptimizer->stopRequested()) {
            return false;
      }

      // 获取当前地图中的关键帧数目
      const int nKPFs = mpAtlas->KeyPoseFramesInMap();

      // Step 3：如果关键帧数目超出最大限制，不插入关键帧 //ERROR
      // mMaxPoseFrames等于Odom输入的频率
      //if(nKPFs>mMaxPoseFrames)
      //{
      //      return false;
      //}      

      // Step 5：查询局部地图线程是否繁忙，当前能否接受新的关键帧
      bool bLocalOptimizingIdle = mpLocalOptimizer->AcceptKeyPoseFrames();

      // Step 7：决策是否需要插入关键帧
      // Step 7.1：很长时间没有插入关键帧，可以插入
      const bool c1a = mCurrentPoseFrame.mnId>=mnLastKeyPoseFrameId+mMaxPoseFrames;
      // Step 7.2：满足插入关键帧的最小间隔并且localMapper处于空闲状态，可以插入
      const bool c1b = ((mCurrentPoseFrame.mnId>=mnLastKeyPoseFrameId+mMinPoseFrames) && bLocalOptimizingIdle);
      // Step 7.3: 距离上一关键帧距离超过10cm，可以插入？
      // Problem18: 插入关键帧时间可长，距离要小


      // 新增的条件c3：IMU+ODOM模式下，并且IMU完成了初始化（隐藏条件），当前帧和上一关键帧之间时间超过0.5秒，则c3=true
      bool c3 = false;
      if(mpLastKeyPoseFrame)
      {
            if (mSensor==System::IMU_ODOM)
            {
                  if ((mCurrentPoseFrame.mTimeStamp-mpLastKeyPoseFrame->mTimeStamp)>=0.5)
                        c3 = true;
            }
      }      

      if((c1a||c1b)||c3)
      {
            // Step 7.6：local mapping空闲时或者正在做imu初始化时可以直接插入，不空闲的时候要根据情况插入
            if(bLocalOptimizingIdle || mpLocalOptimizer->IsInitializing())
            {
                  // 可以插入关键帧
                  return true;
            }
            else
            {
                  mpLocalOptimizer->InterruptBA();
                  if(mSensor=System::IMU_ODOM)
                  {
                        // IMU+ODOM模式下，如队列里没有阻塞太多关键帧，可以插入
                        // tracking插入关键帧不是直接插入，而且先插入到mlNewKeyPoseFrames中，
                        // 然后localOptimizer再逐个pop出来插入到mspKeyPoseFrames
                        if(mpLocalOptimizer->KeyposeframesInQueue()<3)
                              // 队列中的关键帧数目不是很多,可以插入
                              return true;
                        else
                              // 队列中缓冲的关键帧数目太多,暂时不能插入
                              return false;
                  }
                  else
                  {
                        return false;
                  }
            }
      }
      else
            // 不满足上面的条件,自然不能插入关键帧
            return false;
}

/**
 * @brief 创建新的关键帧
 * Step 1：将当前帧构造成关键帧
 * Step 2：将当前关键帧设置为当前帧的参考关键帧
 */
void Tracking::CreateNewKeyPoseFrame()
{
      // 如果局部建图线程正在初始化且没做完或关闭了,就无法插入关键帧
      // 如果是mbOnlyTracking模式，则可以插入关键帧
      if((mpLocalOptimizer->IsInitializing() && !mpAtlas->isImuInitialized()) && !mbOnlyTracking)
            return;

      // Problem7.1 onlytracking模式中一直没有创建新的关键帧
      if(!mpLocalOptimizer->SetNotStop(true) && !mbOnlyTracking)
            return;

      // Step 1：将当前帧构造成关键帧
      KeyPoseFrame* pKPF = new KeyPoseFrame(mCurrentPoseFrame,mpAtlas->GetCurrentMap());

      if(mpAtlas->isImuInitialized()) //  || mpLocalMapper->IsInitializing())
            pKPF->bImu = true;

      pKPF->SetNewBias(mCurrentPoseFrame.mImuBias);

      // Step 2：将当前关键帧设置为当前帧的参考关键帧
      // 在UpdateLocalKeyPoseFrames函数中会将与当前关键帧共视程度最高的关键帧设定为当前帧的参考关键帧
      mpReferenceKPF = pKPF;
      mCurrentPoseFrame.mpReferenceKPF = pKPF;

      if(mpLastKeyPoseFrame)
      {
            pKPF->mPrevKPF = mpLastKeyPoseFrame;
            mpLastKeyPoseFrame->mNextKPF = pKPF;
      }
      else
            std::cout << "Tracking THREAD: No last KPF in KPF creation!!" << std::endl;

      // Reset preintegration from last KPF (Create new object)
      if (mSensor == System::IMU_ODOM)
      {
            mpImuPreintegratedFromLastKPF = new IMU::Preintegrated(pKPF->GetImuBias(),pKPF->mImuCalib);
      }

      // Step 4：插入关键帧
      // 关键帧插入到列表 mlNewKeyPoseFrames中，等待localoptimizing线程处理
      mpLocalOptimizer->InsertKeyPoseFrame(pKPF);

      // 插入好了，允许局部建图停止
      mpLocalOptimizer->SetNotStop(false);

      // 当前帧成为新的关键帧，更新
      mnLastKeyPoseFrameId = mCurrentPoseFrame.mnId;
      mpLastKeyPoseFrame = pKPF;      
}

/**
 * @brief 更新LocalOptim
 * 
 * 局部地图包括： 
 * 1、K1个关键帧、K2个临近关键帧和参考关键帧
 */
void Tracking::UpdateLocalOptim()
{
      // Update
      // 更新局部关键帧
      UpdateLocalKeyPoseFrames();
}

/**
 * @brief 跟踪局部地图函数里，更新局部关键帧
 * 方法是向前遍历足够数量的关键帧作为mvpLocalKeyFrames
 * Step 1：向前遍历更新局部关键帧（mvpLocalKeyFrames）
 */
void Tracking::UpdateLocalKeyPoseFrames()
{
      // Step 1：向前遍历更新局部关键帧（mvpLocalKeyFrames）
      // 先清空局部关键帧
      mvpLocalKeyPoseFrames.clear();
      // 先申请3倍内存，不够后面再加
      mvpLocalKeyPoseFrames.reserve(300*sizeof(map<KeyPoseFrame*,int>));

      if(mSensor == System::IMU_ODOM)
      {
            KeyPoseFrame* tempKeyPoseFrame = mCurrentPoseFrame.mpLastKeyPoseFrame;

            const int Nd = 25; //keyposeframe nums: 5 近似 1s
            for(int i=0; i<Nd; i++)
            {
                  if (!tempKeyPoseFrame)
                        break;
                  if(tempKeyPoseFrame->mnTrackReferenceForPoseFrame!=mCurrentPoseFrame.mnId)
                  {
                        mvpLocalKeyPoseFrames.push_back(tempKeyPoseFrame);
                        tempKeyPoseFrame->mnTrackReferenceForPoseFrame=mCurrentPoseFrame.mnId;
                        tempKeyPoseFrame=tempKeyPoseFrame->mPrevKPF;
                  }
            }
      }
}

/**
 * @brief 整个追踪线程执行复位操作
 */
void Tracking::Reset(bool bLocOptim)
{

}

/**
 * @brief 重置当前活动地图
 */
void Tracking::ResetActiveMap(bool bLocMap)
{
      cout<< "Tracking THREAD: Active map Reseting" << endl;
      if(mpViewer)
      {
            mpViewer->RequestStop();
            while(!mpViewer->isStopped())
                  usleep(3000);
      }  

      Map* pMap = mpAtlas->GetCurrentMap();

      if (!bLocMap)
      {
            cout << "Tracking THREAD: Reseting Local Mapper..." << endl;
            mpLocalOptimizer->RequestResetActiveMap(pMap);
            cout << "Tracking THREAD: done" << endl;
      }  

      // Clear Map (this erase KeyPoseFrames)
      mpAtlas->clearMap();

      mnLastInitPoseFrameId = PoseFrame::nNextId;

      mState = NO_ODOM_YET;

      //mbReadyToInitialize = false;

      // 重置活跃的地图后，更新Tracking类中的mlbLost列表，这个列表用来记录每一帧是否丢失了跟踪
      list<bool> lbLost;
      unsigned int index = mnFirstPoseFrameId;
      cout << "Tracking THREAD: " << "mnFirstPoseFrameId = " << mnFirstPoseFrameId << endl;
      // 找到所有地图中最小的关键帧ID，赋值给index变量。这个变量用来表示当前要处理的帧的ID
      for(Map* pMap : mpAtlas->GetAllMaps())
      {
            if(pMap->GetAllKeyPoseFrames().size() > 0)
            {
                  if(index > pMap->GetLowerKPFID())
                  index = pMap->GetLowerKPFID();
            }
      }

      int num_lost = 0;
      cout << "Tracking THREAD: mnInitialPoseFrameId = " << mnInitialPoseFrameId << endl;
      // 遍历mlbLost列表中的每一个元素，也就是每一帧的跟踪状态。
      //如果index小于mnInitialFrameId，说明这一帧是在当前地图初始化之前的帧，那么就将其跟踪状态原样复制到lbLost列表中。
      //如果index大于等于mnInitialFrameId，说明这一帧是在初始化之后的帧，那么就将其跟踪状态设为true，也就是丢失了跟踪，并且记录丢失的帧数
      for(list<bool>::iterator ilbL = mlbLost.begin(); ilbL != mlbLost.end(); ilbL++)
      {
            if(index < mnInitialPoseFrameId)
                  lbLost.push_back(*ilbL);
            else
            {
                  lbLost.push_back(true);
                  num_lost += 1;
            }

            index++;
      }
      cout << "Tracking THREAD: "<< num_lost << " Frames set to lost" << endl;
      // 在重置活跃的地图后，将所有初始化之后的帧都标记为丢失了跟踪，从而触发重新初始化的流程
      mlbLost = lbLost;

      mnInitialPoseFrameId = mCurrentPoseFrame.mnId;

      mCurrentPoseFrame = PoseFrame();
      mLastPoseFrame = PoseFrame();
      mpReferenceKPF = static_cast<KeyPoseFrame*>(NULL);
      mpLastKeyPoseFrame = static_cast<KeyPoseFrame*>(NULL);

      mbVelocity = false;

      if(mpViewer)
            mpViewer->Release();

      std::cout << "Tracking THREAD: End reseting! " << std::endl;
}

void Tracking::InformOnlyTracking(const bool &flag)
{
    mbOnlyTracking = flag;
}

/**
 * @brief 更新当前帧和上一帧的IMU位姿和速度，以及mlRelativeFramePoses中存储的每一帧相对于参考关键帧的位姿
 * localoptimizing中初始化imu中使用，最开始速度定义于imu初始化时，每个关键帧都根据位移除以时间得到，经过非线性优化保存于KPF中.
 * 之后使用本函数，让上一帧与当前帧分别与他们对应的上一关键帧做速度叠加得到，后面新的poseframe速度由上一个帧速度决定，如果使用匀速模型（大多数情况下），通过imu积分更新速度。
 * 新的关键帧继承于对应帧
 * @param  s 尺度
 * @param  b 初始化后第一帧的偏置
 * @param  pCurrentKeyFrame 当前关键帧
 */
void Tracking::UpdatePoseFrameIMU(const float s, const IMU::Bias &b, KeyPoseFrame* pCurrentKeyPoseFrame)
{
      Map * pMap = pCurrentKeyPoseFrame->GetMap();
      unsigned int index = mnFirstPoseFrameId;

      // 每一帧的参考关键帧
      list<KeyPoseFrame*>::iterator lRit = mlpReferences.begin();
      list<bool>::iterator lbL = mlbLost.begin();  // 对应帧是否跟踪丢失      

      // mlRelativeFramePoses 存放的是Tcr:每一帧相对于参考关键帧的位姿
      // 三个变量一一对应
      // mlRelativeFramePoses用于输出位姿，因此初始化之前里面数据没有尺度，所以要更新下尺度
      for(auto lit=mlRelativeFramePoses.begin(),lend=mlRelativeFramePoses.end();lit!=lend;lit++, lRit++, lbL++)
      {
            if(*lbL)
                  continue;

            KeyPoseFrame* pKPF = *lRit;

            while(pKPF->isBad())
            {
                  pKPF = pKPF->GetParent();
            }

            if(pKPF->GetMap() == pMap)
            {
                  (*lit).translation() *= s;
            }
      }

      // 设置偏置
      mLastBias = b;
      // 设置上一关键帧，如果说mpLastKeyPoseFrame已经是经过添加的新的kpf，而pCurrentKeyPoseFrame还是上一个kf，mpLastKeyPoseFrame直接指向之前的kpf
      mpLastKeyPoseFrame = pCurrentKeyPoseFrame;
      // 更新偏置
      mLastPoseFrame.SetNewBias(mLastBias);
      mCurrentPoseFrame.SetNewBias(mLastBias);

      while(!mCurrentPoseFrame.imuIsPreintegrated())
      {
            // 当前帧需要预积分完毕，这段函数实在localmapping里调用的
            usleep(500);
      }

      // 如果上一帧正好是上一帧的上一关键帧（mLastFrame.mpLastKeyFrame与mLastFrame不可能是一个，可以验证一下）
      if(mLastPoseFrame.mnId == mLastPoseFrame.mpLastKeyPoseFrame->mnFrameId)
      {
            mLastPoseFrame.SetImuPoseVelocity(mLastPoseFrame.mpLastKeyPoseFrame->GetImuRotation(),
                                          mLastPoseFrame.mpLastKeyPoseFrame->GetImuPosition(),
                                          mLastPoseFrame.mpLastKeyPoseFrame->GetVelocity());
      }
      else
      {
            const Eigen::Vector3f Gz(0, 0, -IMU::GRAVITY_VALUE);
            const Eigen::Vector3f twb1 = mLastPoseFrame.mpLastKeyPoseFrame->GetImuPosition();
            const Eigen::Matrix3f Rwb1 = mLastPoseFrame.mpLastKeyPoseFrame->GetImuRotation();
            const Eigen::Vector3f Vwb1 = mLastPoseFrame.mpLastKeyPoseFrame->GetVelocity();
            float t12 = mLastPoseFrame.mpImuPreintegrated->dT;
            // 根据mLastFrame的上一个关键帧的信息（此时已经经过imu初始化了，所以关键帧的信息都是校正后的）以及imu的预积分重新计算上一帧的位姿
            mLastPoseFrame.SetImuPoseVelocity(IMU::NormalizeRotation(Rwb1*mLastPoseFrame.mpImuPreintegrated->GetUpdatedDeltaRotation()),
                                          twb1 + Vwb1*t12 + 0.5f*t12*t12*Gz+ Rwb1*mLastPoseFrame.mpImuPreintegrated->GetUpdatedDeltaPosition(),
                                          Vwb1 + Gz*t12 + Rwb1*mLastPoseFrame.mpImuPreintegrated->GetUpdatedDeltaVelocity());
      }   

      // 当前帧是否做了预积分
      if (mCurrentPoseFrame.mpImuPreintegrated)
      {
            const Eigen::Vector3f Gz(0, 0, -IMU::GRAVITY_VALUE);

            const Eigen::Vector3f twb1 = mCurrentPoseFrame.mpLastKeyPoseFrame->GetImuPosition();
            const Eigen::Matrix3f Rwb1 = mCurrentPoseFrame.mpLastKeyPoseFrame->GetImuRotation();
            const Eigen::Vector3f Vwb1 = mCurrentPoseFrame.mpLastKeyPoseFrame->GetVelocity();
            float t12 = mCurrentPoseFrame.mpImuPreintegrated->dT;

            mCurrentPoseFrame.SetImuPoseVelocity(IMU::NormalizeRotation(Rwb1*mCurrentPoseFrame.mpImuPreintegrated->GetUpdatedDeltaRotation()),
                                          twb1 + Vwb1*t12 + 0.5f*t12*t12*Gz+ Rwb1*mCurrentPoseFrame.mpImuPreintegrated->GetUpdatedDeltaPosition(),
                                          Vwb1 + Gz*t12 + Rwb1*mCurrentPoseFrame.mpImuPreintegrated->GetUpdatedDeltaVelocity());
      }

      mnFirstImuFrameId = mCurrentPoseFrame.mnId;
}


}





